package nape.constraint;
import nape.constraint.TwoConstraint;
import nape.constraint.Constraint;
import nape.geom.VecMath;
import nape.geom.Vec2;
import nape.space.Space;
import nape.phys.PhysObj;
import nape.util.FastMath;
import nape.Const;
















class GrooveJoint extends TwoConstraint {
	
	vec_new(public,ga); vec_new(public,ta);
	vec_new(public,gb); vec_new(public,tb);
	vec_new(public,gn); vec_new(public,tn);
	
	vec_new(public,a2);
	
	vec_new(public,r1);
	vec_new(public,r2);
	
	vec_new(public,jAcc);
	vec_new(public,bias);
	public var clamp:Float;
	public var jMax :Float;
	
	mat_new(public,kt);
	
	public inline function calcNorm() {
		vec_sub(gb,ga,gn);
		vec_normal(gn);
	}
	
	
	
	public function new(obj1:PhysObj,obj2:PhysObj,groove1:Vec2,groove2:Vec2,anchor2:Vec2) {
		super(obj1,obj2);
		
		vec_new(t);
		lanchor(groove1,obj1,ga);
		lanchor(groove2,obj1,gb);
		lanchor(anchor2,obj2,a2);
		
		calcNorm();
		
		vec_set(jAcc,0,0);
	}
	
	
	
	public override function preStep(dt:Float) {
		ranchor(ga,b1,ta); vec_addeq(ta,b1.p);
		ranchor(gb,b1,tb); vec_addeq(tb,b1.p);
		
		ranchor(gn,b1,tn);
		ranchor(a2,b2,r2);
		
		vec_new(bp); vec_add(b2.p,r2,bp);
		var td = vec_cross(bp,tn);
		if(td <= vec_cross(ta,tn)) {
			clamp = 1.0;
			vec_sub(ta,b1.p,r1);
		}else if(td >= vec_cross(tb,tn)) {
			clamp = -1.0;
			vec_sub(tb,b1.p,r1);
		}else  {
			clamp = 0.0;
			var d = vec_dot(ta,tn);
			r1x = (tnx*d)+(tny*td)-b1.px;
			r1y = (tny*d)-(tnx*td)-b1.py;
		}

		KTensor(b1,b2,kt);
		
		jMax = maxForce*dt;
		
		vec_new(d); rdelta(b1,b2,r1,r2,d);
		vec_mul(d,-biasCoef/dt,bias);
		vec_clamp(bias,maxBias);
		
		AppImp(b1,b2,r1,r2,jAcc,);
		
		return false;
	}
	
	
	
	public override function applyImpulse() {
		vec_new(vr); calc_relvel(b1,b2,r1,r2,vr,);
		
		vec_new(j); vec_sub(bias,vr,j);
		mat_transform(kt,j);
		vec_new(jOld); vec_set(jOld,jAcc); vec_addeq(jAcc,j);
		if(clamp*vec_cross(jAcc,tn) <= 0.0) vec_project(jAcc,tn);
		
		if(breakable) { if(vec_lsq(jAcc)>jMax*jMax) return true; }
		vec_clamp(jAcc,jMax);
		
		vec_sub(jAcc,jOld,j);
		AppImp(b1,b2,r1,r2,j,);
		return false;
	}
	
	

	public override function impulse(obj:PhysObj,dst:Vec2) {
		if(obj==b1) vec_set(dst.p,-jAccx,-jAccy);
		else      vec_set(dst.p, jAcc);
	}
}